# coding=utf-8
# 用opencv实现兑换站识别
import math
import cv2
import numpy as np
import time
import threading
from util_3 import *

import sys
sys.path.append('/opt/ros/melodic/lib/python2.7/dist-packages')
sys.path.append('/home/tjurm/.local/lib/python3.6/site-packages')

#ros发布的部分
# import rospy
# from geometry_msgs.msg import Point, Vector3

info = [[0,0,0],[0,0,0],[0,0,0]]
result = info

color_image = None

# lock = None

# ROS_publish_topic,main中需要调用此函数
# def publish_geometry():

#     rospy.init_node('publish_geometry', anonymous=True,  disable_signals=True)
#     rate = rospy.Rate(10) # 10hz

#     # 创建四个消息发布者
#     center_pub = rospy.Publisher('center', Point, queue_size=10)
#     unit_vec1_pub = rospy.Publisher('unit_vec1', Point, queue_size=10)
#     unit_vec2_pub = rospy.Publisher('unit_vec2', Point, queue_size=10)
    
#     while True:
#         try:
#             # (not rospy.is_shutdown()) and 
#             # lock.acquire()
#             center, unit_vec1, unit_vec2  = info[0], info[1], info[2]
#             # lock.release
#             print("send", end=" ")
#             # 发布center
#             center_msg = Point()
#             center_msg.x = -center[0]
#             center_msg.y = -center[1]
#             center_msg.z = center[2]
#             center_pub.publish(center_msg)

#             # 发布unit_vec1
#             unit_vec1_msg = Point()
#             unit_vec1_msg.x = -unit_vec1[0]
#             unit_vec1_msg.y = -unit_vec1[1]
#             unit_vec1_msg.z = unit_vec1[2]
#             unit_vec1_pub.publish(unit_vec1_msg)

#             # 发布unit_vec2
#             unit_vec2_msg =  Point()
#             unit_vec2_msg.x = -unit_vec2[0]
#             unit_vec2_msg.y = -unit_vec2[1]
#             unit_vec2_msg.z = unit_vec2[2]
#             unit_vec2_pub.publish(unit_vec2_msg)

#             rate.sleep()
#             # print(info)
#         except:
#             print("error")

##############################################################################################################################

if __name__ == '__main__':

    # 参数设置
    W, H = 640, 480 # 图像宽高
    # W, H = 1280, 720 # 图像宽高
    FPS = 30 # 帧率
    MAX_LENGTH = 1000 # 区分正面和侧面轮廓所使用的空间长度
    RED = (0, 0, 255) # 红色
    BLUE = (255, 0, 0) # 蓝色
    GREEN = (0, 255, 0) # 绿色
    # Red-True, Blue-False
    red_or_blue = True
    # red_or_blue = False

    # 设置hsv阈值
    lower_blue = np.array([87, 96, 160])
    # lower_blue = np.array([100, 43, 46])
    upper_blue = np.array([124, 255, 255])
    # upper_blue = np.array([124, 255, 255])

    lower_red = np.array([0, 33, 40])
    upper_red = np.array([30, 255, 255])
    # 创建一个管道
    pipeline = rs.pipeline() 
    # 配置要流式传输的管道
    config = rs.config()
    config.enable_stream(rs.stream.color, W, H, rs.format.bgr8, 30) # 8位rgb图像
    config.enable_stream(rs.stream.depth, W, H, rs.format.z16, 30) # 16位深度图像
    # 左右双目
    config.enable_stream(rs.stream.infrared, 1, W, H, rs.format.y8, 30) # 8位左红外图像
    config.enable_stream(rs.stream.infrared, 2, W, H, rs.format.y8, 30) # 8位右红外图像
    align = rs.align(rs.stream.color)
    # 视野大，但有残缺
    # align = rs.align(rs.stream.depth)
    # 开始传输帧
    profile = pipeline.start(config)
    # 更改rgb相机的曝光值
    color_sensor = profile.get_device().first_color_sensor()
    depth_sensor = profile.get_device().first_depth_sensor()
    # 关闭自动曝光
    color_sensor.set_option(rs.option.enable_auto_exposure, 0)
    depth_sensor.set_option(rs.option.enable_auto_exposure, 0)
    # 设置曝光值
    color_sensor.set_option(rs.option.exposure, 25)
    depth_sensor.set_option(rs.option.exposure, 15000)
    # 
    depth_sensor = profile.get_device().first_depth_sensor()
    # 
    depth_scale = depth_sensor.get_depth_scale()


    # sender = threading.Thread(target=publish_geometry)
    # sender.start()
    # lock = threading.Lock()

##############################################################################################################################
# 对视频进行循环处理
    times = 0
    #time.sleep(90)
    print('90s')

    while True:
        # times += 1
        # if(times > 1000):
        #     break
        time1 = time.time()
        # 等待开启通道
        frames = pipeline.wait_for_frames()
        # 将深度图和RGB图对齐
        frames = align.process(frames)
        # 获得RGB图像
        color_frame = frames.get_color_frame()
        # 未获得RGB图像
        if not color_frame:
            continue
        # 获得深度图像
        depth_frame = frames.get_depth_frame()
        # 把图像像素转化为数组
        color_image = np.asanyarray(color_frame.get_data())
        depth_image = np.asanyarray(depth_frame.get_data())
        # 深度图像转化为伪彩色图像(alpha数值越大对比度色差越大)
        depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.21), cv2.COLORMAP_JET)
        # 转换到HSV (颜色空间转换) (色调H，饱和度S，亮度V)
        hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV)
        # 根据阈值构建掩模 (将在两个阈值内的像素值设置为白色，而不在阈值区间内的像素值设置为黑色)
        if red_or_blue:
            binary = cv2.inRange(hsv, lower_red, upper_red)
        else:
            binary = cv2.inRange(hsv, lower_blue, upper_blue)
        # 形态学处理
        kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
        # binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel)
        # binary = cv2.morphologyEx(binary, cv2.MORPH_CLOSE, kernel)
        # 放慢视频播放速度
        # time.sleep(0.1)
        
        # 寻找轮廓
        contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        # 正面轮廓
        front_contours = []

        # 求最大轮廓面积
        max_area = 0
        for i in range(len(contours)):
            area = cv2.contourArea(contours[i])
            if area > max_area:
                max_area = area

        # 遍历轮廓分类
        for i in range(len(contours)):
            # 筛去面积过小的轮廓
            area = cv2.contourArea(contours[i])
            if area < max_area / 7 or area < 10:
                continue

            # 筛去面积占外接矩形比例过大的轮廓
            rect =  cv2.minAreaRect(contours[i])
            rect_area = rect[1][0] * rect[1][1]
            if area > 0.9 * rect_area:
                continue

            # 筛去外接矩形长宽比太大的轮廓
            if rect[1][0] / rect[1][1] > 3 or rect[1][1] / rect[1][0] > 3:
                continue

            # 正面轮廓
            front_contours.append(contours[i])

        # 保留最大的4个正面轮廓
        while len(front_contours) > 4:
            min_area = 100000
            min_idx = 0
            for i in range(len(front_contours)):
                rect =  cv2.minAreaRect(front_contours[i])
                area = rect[1][0] * rect[1][1]
                if area < min_area:
                    min_area = area
                    min_idx = i
            front_contours.pop(min_idx)


##############################################################################################################################
# 正面轮廓处理
        if len(front_contours) == 4:

            center_12 = [0, 0]
            points_1= find_coutour_points(front_contours[0], color_image)
            for p in points_1:
                center_12[0] += p[0]
                center_12[1] += p[1]
            points_2 = find_coutour_points(front_contours[1], color_image)
            for p in points_2:
                center_12[0] += p[0]
                center_12[1] += p[1]
            points_3 = find_coutour_points(front_contours[2], color_image)
            for p in points_3:
                center_12[0] += p[0]
                center_12[1] += p[1]
            points_4 = find_coutour_points(front_contours[3], color_image)
            for p in points_4:
                center_12[0] += p[0]
                center_12[1] += p[1]
            
            center_12[0] /= 12
            center_12[1] /= 12

            
            # 最小二乘法求L形两边的交点
            cross1 = contour_point(front_contours[0], color_image, center_12)
            cross2 = contour_point(front_contours[1], color_image, center_12)
            cross3 = contour_point(front_contours[2], color_image, center_12)
            cross4 = contour_point(front_contours[3], color_image, center_12)

            if cross1 == None or cross2 == None or cross3 == None or cross4 == None:
                print("cross is None")
                continue
        
            points = [cross1, cross2, cross3, cross4]
            out_of_range = False
            for p in points:
                if p[0] < 0 or p[0] > 640 or p[1] < 0 or p[1] > 480:
                    print("cross out of range")
                    out_of_range = True

            if out_of_range:
                continue

            # cv2.circle(color_image, (int(center_12[0]), int(center_12[1])), 5, (0, 0, 255), -1)

            # points = []
            # for i in range(len(points_1)):
            #     for j in range(i + 1, len(points_1)):
            #         vec1 = np.array(points_1[i]) - np.array(center_12)
            #         vec2 = np.array(points_1[j]) - np.array(center_12)
            #         vec = np.array(points_1[3 - i - j]) - np.array(center_12)
            #         if np.cross(vec, vec1) * np.cross(vec, vec2) < 0:
            #             points.append(points_1[3 - i - j])
            #             break
            # for i in range(len(points_2)):
            #     for j in range(i + 1, len(points_2)):
            #         vec1 = np.array(points_2[i]) - np.array(center_12)
            #         vec2 = np.array(points_2[j]) - np.array(center_12)
            #         vec = np.array(points_2[3 - i - j]) - np.array(center_12)
            #         if np.cross(vec, vec1) * np.cross(vec, vec2) < 0:
            #             points.append(points_2[3 - i - j])
            #             break

            # for i in range(len(points_3)):
            #     for j in range(i + 1, len(points_3)):
            #         vec1 = np.array(points_3[i]) - np.array(center_12)
            #         vec2 = np.array(points_3[j]) - np.array(center_12)
            #         vec = np.array(points_3[3 - i - j]) - np.array(center_12)
            #         if np.cross(vec, vec1) * np.cross(vec, vec2) < 0:
            #             points.append(points_3[3 - i - j])
            #             break

            # for i in range(len(points_4)):
            #     for j in range(i + 1, len(points_4)):
            #         vec1 = np.array(points_4[i]) - np.array(center_12)
            #         vec2 = np.array(points_4[j]) - np.array(center_12)
            #         vec = np.array(points_4[3 - i - j]) - np.array(center_12)
            #         if np.cross(vec, vec1) * np.cross(vec, vec2) < 0:
            #             points.append(points_4[3 - i - j])
            #             break

            try:
                result = printResult(color_image, depth_frame, points, 4)
            except:
                print("out of range")
       

        elif False and len(front_contours) == 3:

            # 最小二乘法求L形两边的交点
            cross1 = contour_point(front_contours[0], color_image)
            cross2 = contour_point(front_contours[1], color_image)
            cross3 = contour_point(front_contours[2], color_image)

            if cross1 == None or cross2 == None or cross3 == None:
                print("cross is None")
                continue

            points = [cross1, cross2, cross3]

            out_of_range = False
            for p in points:
                if p[0] < 0 or p[0] > 640 or p[1] < 0 or p[1] > 480:
                    print("cross out of range")
                    out_of_range = True

            if out_of_range:
                continue

            # 中心点坐标
            x = (cross1[0] + cross2[0]) / 2
            y = (cross1[1] + cross2[1]) / 2
            
            # 绘制中心点
            cv2.circle(color_image, (int(x), int(y)), 5, RED, -1)
            cv2.line(color_image, cross1, cross2, (0, 255, 255), 1)
            cv2.line(color_image, cross3, cross2, (0, 255, 255), 1)
            cv2.line(color_image, cross1, cross3, (0, 255, 255), 1)

            # 输出结果
            result = printResult(color_image, depth_frame, points, lock, 3)

        # print("lock2 ", lock)
        # lock.acquire()
        info = result
        print("info: ", info)
        print("")
        # lock.release()

        # 识别失败
        if len(front_contours) > 4:
            print("too many contours")
        elif len(front_contours) < 3:
            print("too few contours")

        # 显示图像
        window_name = "auto"
        cv2.namedWindow(window_name, 0)
        # cv2.resizeWindow(window_name, (1920, 1080))
        # cv2.resizeWindow(window_name, (1280, 960))
        cv2.resizeWindow(window_name, (640, 480))
        cv2.moveWindow(window_name, 0,0)
        cv2.imshow(window_name, color_image)
        # cv2.imshow('color_image', color_image) 
        # cv2.imshow('depth_image', depth_colormap)
        # cv2.imshow('binary', binary)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

        time2 = time.time()
        print("time: ", time2 - time1)

    info[0][0] = 100000000


    # 杀死sender子线程
    # sender.kill()

    cv2.destroyAllWindows()
